function particles = particle_update(particles, observations, observed_ids, R)
% 粒子观测更新（改进版：使用数据关联）
% 输入: particles-粒子数组, observations-观测值, observed_ids-不再使用，保留兼容性, R-观测噪声
% 输出: particles-更新后的粒子

    N = length(particles);
    
    % 如果没有观测，直接返回
    if isempty(observations)
        return;
    end
    
    % 全局路标ID计数器（持久变量）
    persistent global_landmark_counter;
    if isempty(global_landmark_counter)
        global_landmark_counter = 1;
    end
    
    for i = 1:N
        particle_weight = 1.0;
        
        % 提取粒子的路标信息
        landmarks = [];
        if ~isempty(particles(i).landmarks)
            for k = 1:length(particles(i).landmarks)
                landmarks = [landmarks; particles(i).landmarks(k).mean'];
            end
        end
        
        % 数据关联（使用自适应阈值，FastSLAM中每个粒子独立）
        [associations, is_new] = data_association(observations, particles(i).pose, ...
                                                  landmarks, particles(i).landmark_ids, [], R, []);
        
        % 处理每个观测
        for j = 1:size(observations, 2)
            z = observations(:, j);
            
            if is_new(j)
                % 新路标：初始化
                new_id = global_landmark_counter;
                global_landmark_counter = global_landmark_counter + 1;
                particles(i) = initialize_landmark_particle(particles(i), new_id, z, R);
                % 新路标权重设为固定值
                particle_weight = particle_weight * 0.1;
            else
                % 已知路标：EKF更新
                lm_idx = associations(j);
                [particles(i), w] = update_landmark_particle(particles(i), lm_idx, z, R);
                particle_weight = particle_weight * w;
            end
        end
        
        % 更新粒子权重
        particles(i).weight = particles(i).weight * particle_weight;
    end
    
    % 归一化权重
    total_weight = sum([particles.weight]);
    if total_weight > 0
        for i = 1:N
            particles(i).weight = particles(i).weight / total_weight;
        end
    else
        % 所有权重为0，重置为均匀分布
        for i = 1:N
            particles(i).weight = 1/N;
        end
    end
end

function particle = initialize_landmark_particle(particle, lm_id, z, R)
% 初始化新路标
    pose = particle.pose;
    range = z(1);
    bearing = z(2);
    
    % 计算路标全局位置
    mx = pose(1) + range * cos(pose(3) + bearing);
    my = pose(2) + range * sin(pose(3) + bearing);
    
    % 初始化协方差
    theta = pose(3);
    Jx = [1, 0; 0, 1];  % 对位置的雅可比（简化）
    Jz = [cos(theta + bearing), -range * sin(theta + bearing);
          sin(theta + bearing),  range * cos(theta + bearing)];
    
    P = Jz * R * Jz' + eye(2) * 0.1;  % 添加小的初始不确定性
    
    % 创建路标
    landmark = struct();
    landmark.mean = [mx; my];
    landmark.cov = P;
    
    % 添加到粒子
    particle.landmarks = [particle.landmarks; landmark];
    particle.landmark_ids = [particle.landmark_ids; lm_id];
end

function [particle, weight] = update_landmark_particle(particle, lm_idx, z, R)
% 更新已知路标
    pose = particle.pose;
    lm = particle.landmarks(lm_idx);
    
    % 预测观测
    dx = lm.mean(1) - pose(1);
    dy = lm.mean(2) - pose(2);
    q = dx^2 + dy^2;
    
    if q < 1e-6
        weight = 0.01;
        return;
    end
    
    range_pred = sqrt(q);
    bearing_pred = wrapToPi(atan2(dy, dx) - pose(3));
    z_pred = [range_pred; bearing_pred];
    
    % 观测雅可比
    H = zeros(2, 2);
    H(1, 1) = dx / range_pred;
    H(1, 2) = dy / range_pred;
    H(2, 1) = -dy / q;
    H(2, 2) = dx / q;
    
    % 创新
    innovation = z - z_pred;
    innovation(2) = wrapToPi(innovation(2));
    
    % 创新协方差
    S = H * lm.cov * H' + R;
    
    % 计算权重（似然）
    weight = mvnpdf(innovation, [0; 0], S);
    weight = max(weight, 1e-10);  % 避免权重为0
    
    % 卡尔曼增益
    K = lm.cov * H' / S;
    
    % 更新路标
    lm.mean = lm.mean + K * innovation;
    lm.cov = (eye(2) - K * H) * lm.cov;
    lm.cov = (lm.cov + lm.cov') / 2;  % 确保对称
    
    particle.landmarks(lm_idx) = lm;
end

